#include "my_servo.h"

float Debug_Servo1=0;//48.8;
float Debug_Servo2=0;//3.5;
Gimbal_Control_Parm gimbal_control_parm=
{
		.current_index=0,
		.step = 0.1f,
    .threshold = 0.10f,
		.stable_count=0,
};
Point Gimbal_Corner_Point[5]=
{
		{15.84, 58.68},			//左上  yaw,pitch
		{0.18, 	58.68},			//右上
		{0.18, 	26.64},			//右下
		{14.04, 25.92},			//左下
		{6.84, 	46.80},			// 中心
  
};

Gimbal_Angle gimbal_angle;
Gimbal_Control_Pid gimbal_pitch;
Gimbal_Control_Pid gimbal_yaw;


 void Gimbal_Pid_Init()
{
	//pitch 轴  1
	gimbal_pitch.Kp=0.40;
	gimbal_pitch.Kd=0.02;
	gimbal_pitch.Ki=0.10;
	

	gimbal_pitch.Target=0;
	gimbal_pitch.Actual=0;
	gimbal_pitch.Error0=0;
	gimbal_pitch.Error1=0;
	gimbal_pitch.ErrorInt=0;
	gimbal_pitch.Der_Out=0;
	gimbal_pitch.Out=0;
	//yaw轴    2
	gimbal_yaw.Kp=0.40;
	gimbal_yaw.Kd=0.02;
	gimbal_yaw.Ki=0.10;
	

	gimbal_yaw.Target=0;
	gimbal_yaw.Actual=0;
	gimbal_yaw.Error0=0;
	gimbal_yaw.Error1=0;
	
	gimbal_yaw.ErrorInt=0;
	gimbal_yaw.Der_Out=0;
	gimbal_yaw.Out=0;
	
}
static void gimbal_out_limit(float *out,float limit)
{
		if(*out>limit)*out=limit;
		if(*out<-limit)*out=-limit;
}
void Gimbal_Pid()
{
    // ==== 滤波相关静态变量 ====
    static float filtered_pitch = 0, filtered_yaw = 0;
    static float filtered_d_pitch = 0, filtered_d_yaw = 0;
    #define LPF(alpha, new_val, last_val) ((alpha)*(new_val) + (1.0f - (alpha))*(last_val))

    // ==== 1. 读取并滤波当前角度 ====
    filtered_pitch = LPF(0.3f, gimbal_angle.pitch, filtered_pitch);
    filtered_yaw   = LPF(0.3f, gimbal_angle.yaw, filtered_yaw);
    
    gimbal_pitch.Actual = filtered_pitch;
    gimbal_yaw.Actual   = filtered_yaw;

    // ==== 2. 计算误差 ====
    gimbal_pitch.Error0 = gimbal_pitch.Target - gimbal_pitch.Actual;
    gimbal_yaw.Error0   = gimbal_yaw.Target   - gimbal_yaw.Actual;
	
	
		// pitch/yaw目标角度微调：如果变化太小，就强制加微量扰动
//		if (fabs(gimbal_pitch.Error0) < 0.36f)
//		{
//				gimbal_pitch.Target += (gimbal_pitch.Target > gimbal_pitch.Actual) ? 0.36f : -0.36f;
//		}
//		if (fabs(gimbal_yaw.Error0) < 0.36f)
//		{
//				gimbal_yaw.Target += (gimbal_yaw.Target > gimbal_yaw.Actual) ? 0.36f : -0.36f;
//		}


    // ==== 3. 积分项处理（防震荡限幅） ====
    gimbal_pitch.ErrorInt = gimbal_pitch.Ki * gimbal_pitch.Error0;
    if (gimbal_pitch.ErrorInt > 5)  gimbal_pitch.ErrorInt = 5;
    if (gimbal_pitch.ErrorInt < -5) gimbal_pitch.ErrorInt = -5;

    gimbal_yaw.ErrorInt = gimbal_yaw.Ki * gimbal_yaw.Error0;
    if (gimbal_yaw.ErrorInt > 5)  gimbal_yaw.ErrorInt = 5;
    if (gimbal_yaw.ErrorInt < -5) gimbal_yaw.ErrorInt = -5;

    // ==== 4. D项二阶差分计算 + 滤波 ====
    float d_raw_pitch = gimbal_pitch.Kd * (gimbal_pitch.Error0 - 2.0f * gimbal_pitch.Error1 + gimbal_pitch.Error2);
    float d_raw_yaw   = gimbal_yaw.Kd   * (gimbal_yaw.Error0   - 2.0f * gimbal_yaw.Error1   + gimbal_yaw.Error2);

    filtered_d_pitch = LPF(0.3f, d_raw_pitch, filtered_d_pitch);
    filtered_d_yaw   = LPF(0.3f, d_raw_yaw, filtered_d_yaw);

    gimbal_pitch.Pid_D = filtered_d_pitch;
    gimbal_yaw.Pid_D   = filtered_d_yaw;

    // ==== 5. 计算增量式PID输出 ====
    gimbal_pitch.Der_Out = gimbal_pitch.Kp * (gimbal_pitch.Error0 - gimbal_pitch.Error1)
                         + gimbal_pitch.ErrorInt
                         + gimbal_pitch.Pid_D;

    gimbal_yaw.Der_Out = gimbal_yaw.Kp * (gimbal_yaw.Error0 - gimbal_yaw.Error1)
                       + gimbal_yaw.ErrorInt
                       + gimbal_yaw.Pid_D;

    gimbal_out_limit(&gimbal_pitch.Der_Out, 10);
    gimbal_out_limit(&gimbal_yaw.Der_Out, 10);

    // ==== 6. 防止死区无法移动（微输出推力） ====
    if (fabs(gimbal_pitch.Der_Out) < 0.32f && fabs(gimbal_pitch.Error0) > 2.0f)
        gimbal_pitch.Der_Out = (gimbal_pitch.Error0 > 0) ? 0.32f : -0.32f;
    
    if (fabs(gimbal_yaw.Der_Out) < 0.32f && fabs(gimbal_yaw.Error0) > 2.0f)
        gimbal_yaw.Der_Out = (gimbal_yaw.Error0 > 0) ? 0.32f : -0.32f;
		
		
		if(fabs(gimbal_pitch.Der_Out)<0.0025f){gimbal_pitch.Der_Out =0;}
		if(fabs(gimbal_yaw.Der_Out)<0.0025f){gimbal_yaw.Der_Out =0;}

    // ==== 7. 保存历史误差 ====
    gimbal_pitch.Error2 = gimbal_pitch.Error1;
    gimbal_pitch.Error1 = gimbal_pitch.Error0;

    gimbal_yaw.Error2 = gimbal_yaw.Error1;
    gimbal_yaw.Error1 = gimbal_yaw.Error0;

    // ==== 8. 输出更新 ====
    gimbal_pitch.Out += gimbal_pitch.Der_Out;
    gimbal_yaw.Out   += gimbal_yaw.Der_Out;

    if (gimbal_pitch.Out > 180) Debug_Servo1 = 180;
    else if (gimbal_pitch.Out < 0) Debug_Servo1 = 0;
    else Debug_Servo1 = gimbal_pitch.Out;

    if (gimbal_yaw.Out > 180) Debug_Servo2 = 180;
    else if (gimbal_yaw.Out < 0) Debug_Servo2 = 0;
    else Debug_Servo2 = gimbal_yaw.Out;

    // ==== 9. PWM 输出 ====
    pwm_set_duty(SERVO_MOTOR1_PWM, (uint32)SERVO_MOTOR_DUTY(Debug_Servo1));
    pwm_set_duty(SERVO_MOTOR2_PWM, (uint32)SERVO_MOTOR_DUTY(Debug_Servo2));
}

void Gimbal_Point_Follow_Task()
{
    // 当前目标点
    Point target_point = Gimbal_Corner_Point[gimbal_control_parm.current_index];

    // 设置为 PID 的目标值
    gimbal_pitch.Target = target_point.y;  // pitch 是上下方向
    gimbal_yaw.Target   = target_point.x;  // yaw 是左右方向

    // 判断是否达到目标点（pitch/yaw 误差都足够小）
    float pitch_error = fabs(gimbal_pitch.Target - gimbal_pitch.Actual);
    float yaw_error   = fabs(gimbal_yaw.Target   - gimbal_yaw.Actual);
	
	
		if (pitch_error < gimbal_control_parm.threshold && yaw_error < gimbal_control_parm.threshold)
		{
				gimbal_control_parm.stable_count = 0;
				gimbal_control_parm.current_index++;
				if (gimbal_control_parm.current_index >= (sizeof(Gimbal_Corner_Point) / sizeof(Gimbal_Corner_Point[0])) - 1)
				{
						gimbal_control_parm.current_index = 0;  // 循环
				}
				
		}

    // 进入 PID 控制逻辑
    Gimbal_Pid();  
}
void Gimbal_Pid_Reset()
{
    gimbal_pitch.Error0 = gimbal_pitch.Error1 = gimbal_pitch.Error2 = 0;
    gimbal_pitch.ErrorInt = 0;
    gimbal_pitch.Der_Out = 0;
    gimbal_pitch.Pid_D = 0;

    gimbal_yaw.Error0 = gimbal_yaw.Error1 = gimbal_yaw.Error2 = 0;
    gimbal_yaw.ErrorInt = 0;
    gimbal_yaw.Der_Out = 0;
    gimbal_yaw.Pid_D = 0;
}
/**
 * @brief 将 PWM 占空比值转换为舵机角度
 * * @param duty  输入的PWM占空比值 (范围 0 ~ PWM_DUTY_MAX)
 * @return float 计算出的舵机角度值
 */
static inline float servo_duty_to_angle(uint32_t duty)
{
    // 1. 将占空比值转换回脉冲宽度(毫秒)
    float pulse_width_ms = (float)duty / (float)PWM_DUTY_MAX * (1000.0f / (float)SERVO_MOTOR_FREQ);

    // 2. 将脉冲宽度(毫秒)转换回角度
    float angle = (pulse_width_ms - 0.5f) * 90.0f;

    return angle;
}

void Set_Gimbal_Angle()
{
		gimbal_pitch.Target =48.8;
		gimbal_yaw.Target 	=3.5;
}
void get_gimbal_angle()
{
	gimbal_angle.pitch = servo_duty_to_angle(pwm_get_duty(SERVO_MOTOR1_PWM));
	gimbal_angle.yaw   = servo_duty_to_angle(pwm_get_duty(SERVO_MOTOR2_PWM));
}
void my_servo_init()
{
    pwm_init(SERVO_MOTOR1_PWM, SERVO_MOTOR_FREQ, 0);
    pwm_init(SERVO_MOTOR2_PWM, SERVO_MOTOR_FREQ, 0);
    pwm_init(SERVO_MOTOR3_PWM, SERVO_MOTOR_FREQ, 0);
	
		Gimbal_Pid_Init();
}
void Servo_Control()
{
    pwm_set_duty(SERVO_MOTOR1_PWM, (uint32)SERVO_MOTOR_DUTY(Debug_Servo1));
    pwm_set_duty(SERVO_MOTOR2_PWM, (uint32)SERVO_MOTOR_DUTY(Debug_Servo2));
}



